#ifndef BTL_CAMERA_CAMERAMODELATAN
#define BTL_CAMERA_CAMERAMODELATAN

#include <btl/Camera/CameraModel.hpp>

namespace btl {
namespace camera {

class CameraModelATAN : public CameraModelPinhole, public CameraModelDistortedBidirectional
{
   public:

      CameraModelATAN(
         int imageResolutionX, int imageResolutionY,
         double focalLength,
         double principlePointX, double principlePointY,
         double omega);
      CameraModelATAN(
         const Eigen::Vector2i& imageResolution,
         double focalLength,
         const Eigen::Vector2d& principlePoint,
         double omega);
      CameraModelATAN(
         int imageResolutionX, int imageResolutionY,
         double focalLengthX, double focalLengthY,
         double principlePointX, double principlePointY,
         double omega);
      CameraModelATAN(
         const Eigen::Vector2i& imageResolution,
         const Eigen::Vector2d& focalLength,
         const Eigen::Vector2d& principlePoint,
         double omega);

      ~CameraModelATAN();

      double getOmega() const;
      void setOmega(double w);

      Eigen::Vector2d distortPoint(const Eigen::Vector2d& undistorted);
      Eigen::Vector2d undistortPoint(const Eigen::Vector2d& distorted);

   private:
      double _omega;
      double _invOmega;
      double _twoTanHalfOmega;
      double _invTwoTanHalfOmega;
};

} //namespace camera
} //namespace btl

namespace btl {

   using camera::CameraModelATAN;

} //namespace btl

namespace btl {
namespace camera {

inline CameraModelATAN::CameraModelATAN(
      int imageResolutionX, int imageResolutionY,
      double focalLength,
      double principlePointX, double principlePointY,
      double omega):
   CameraModelPinhole(imageResolutionX, imageResolutionY, focalLength, principlePointX, principlePointY)
{
   setOmega(omega);
}

inline CameraModelATAN::CameraModelATAN(
      const Eigen::Vector2i& imageResolution,
      double focalLength,
      const Eigen::Vector2d& principlePoint,
      double omega):
   CameraModelPinhole(imageResolution, focalLength, principlePoint)
{
   setOmega(omega);
}

inline CameraModelATAN::CameraModelATAN(
      int imageResolutionX, int imageResolutionY,
      double focalLengthX, double focalLengthY,
      double principlePointX, double principlePointY,
      double omega):
   CameraModelPinhole(imageResolutionX, imageResolutionY, focalLengthX, focalLengthY, principlePointX, principlePointY)
{
   setOmega(omega);
}

inline CameraModelATAN::CameraModelATAN(
      const Eigen::Vector2i& imageResolution,
      const Eigen::Vector2d& focalLength,
      const Eigen::Vector2d& principlePoint,
      double omega):
   CameraModelPinhole(imageResolution, focalLength, principlePoint)
{
   setOmega(omega);
}

inline CameraModelATAN::~CameraModelATAN()
{}

inline double CameraModelATAN::getOmega() const
{
   return _omega;
}

inline void CameraModelATAN::setOmega(double w)
{
   _omega = w;
   if(w != 0.0)
   {
      _invOmega = 1.0 / w;
      _twoTanHalfOmega = 2.0 * tan(w / 2.0);
      _invTwoTanHalfOmega = 1.0 / _twoTanHalfOmega;
   }
   else
   {
      _invOmega = 0.0;
      _twoTanHalfOmega = 0.0;
      _invTwoTanHalfOmega = 0.0;
   }
}

inline Eigen::Vector2d CameraModelATAN::distortPoint(const Eigen::Vector2d& pu)
{
   if(_omega != 0.0)
   {
      double ru = pu.norm();
      double rd = _invOmega * atan(ru * _twoTanHalfOmega);
      double scale = (ru > 0.00001) ? rd / ru : 1.0;
      return _principlePoint + ((pu - _principlePoint) * scale);
   }
   else
      return pu;
}

inline Eigen::Vector2d CameraModelATAN::undistortPoint(const Eigen::Vector2d& pd)
{
   if(_omega != 0.0)
   {
      double rd = pd.norm();
      double ru = _invTwoTanHalfOmega * tan(rd * _omega);
      double scale = (rd > 0.00001) ? ru / rd : 1.0;
      return _principlePoint + ((pd - _principlePoint) * scale);
   }
   else
      return pd;
}

} // namespace camera
} // namespace btl

#endif // BTL_CAMERA_CAMERAMODELATAN
